/*
 * lane_model_fitting.cpp
 *
 *  Created on: May 11, 2018
 *      Author: geoff
 */
#include "lane_model_fitting.h"
#include <opencv2/highgui/highgui.hpp>
#include "calculate_tools.h"
#include "custom_hough_lines.h"
#include "debug_show.h"

//#define DEBUG_BLACKPM
//#define DEBUG_HOUGH
//#define DEBUG_CLUSTER
//#define DEBUG_TEST

namespace rmd {

LaneModelFitting::LaneModelFitting() { lane_kalman_ = CKalman::Instance(); }

LaneModelFitting::~LaneModelFitting() {}

bool SortByDownExtendPointX(const KeyLine& v1, const KeyLine& v2) {
  return v1.ptDownExtendPoint.x < v2.ptDownExtendPoint.x;
}

void LaneModelFitting::SetDataFrameToClass(DataFrame* data_frame,
                                           cv::Mat& pm_black) {
  data_frame_ = data_frame;
  pm_gray_image = data_frame->process_grayimage;
  pm_black_image = pm_black;

  image_width = kImageWidth;
  image_height = kImageHeight;

#ifdef DEBUG_HOUGH
  data_frame->process_image.copyTo(show_hough_image_);
#endif

#ifdef DEBUG_TEST
  data_frame->process_image.copyTo(show_cluster_image_);
  data_frame->process_image.copyTo(show_origin_image_);
#endif

  far_line_ = data_frame->far_line;
  middle_start_line_ = data_frame->middle_start_line;
  start_line_ = data_frame->start_line;
  end_line_ = data_frame->end_line;

  adapt_vanishing_point = data_frame->adapt_vanishing_point;
  constant_vanishing_point = data_frame->constant_vanishing_point;

  lane_line_fitting_width_ = data_frame->lane_line_fitting_width;

  // pri_lane_lines = data_frame->pri_lane_lines;
  lindex = data_frame->lIndex;
}

long LaneModelFitting::GetIindex() { return lindex; }

void LaneModelFitting::SetAutoLdVps(cv::Point left_up, cv::Point left_down,
                                    cv::Point right_up, cv::Point right_down) {
  float extend_scale = 1;
  Point2f pt = CalcuateTools::GetInstance()->GetCrossPoint(
      left_up, left_down, right_up, right_down);

  if (pt.x > data_frame_->left_boundary.x &&
      pt.x < data_frame_->right_boundary.x && pt.y > kImageHeight / 3 &&
      pt.y < kImageHeight - kRoiHeight / 2 &&
      abs(pt.x - data_frame_->user_vanishing_point.x) <
          extend_scale * data_frame_->x_Length &&
      abs(pt.y - data_frame_->user_vanishing_point.y) <
          extend_scale * data_frame_->y_Length) {
    INITLDVP tInitldvp;
    tInitldvp.x = pt.x;
    tInitldvp.y = pt.y;
    tInitldvp.left_point[0] = left_up;
    tInitldvp.left_point[1] = left_down;
    tInitldvp.right_point[0] = right_up;
    tInitldvp.right_point[1] = right_down;
    data_frame_->auto_ld_vps.push_back(tInitldvp);
    //    std::cout << "auto_ld_vps:" << data_frame_->auto_ld_vps.size() <<
    //    std::endl;
  }
}

void LaneModelFitting::UpdateAuto(DataFrame* data_frame, cv::Mat& pm_black) {
  SetDataFrameToClass(data_frame, pm_black);
  std::vector<cv::Vec4i> hough_lines =
      GetFieldLinesV0(middle_start_line_, end_line_);
  std::vector<KeyLine> key_lane_lines = AutoClusterLine(hough_lines);

  if (hough_lines.size() > kFitMaxHoughCount) {
    return;
  }

#ifdef DEBUG_TEST
  for (int i = 0; i < key_lane_lines.size(); i++) {
    cv::Point pt1 = key_lane_lines[i].ptUpExtendPoint;    // ptUpPoint;
    cv::Point pt2 = key_lane_lines[i].ptDownExtendPoint;  // ptDownPoint;
    cv::line(show_cluster_image_, cv::Point(pt1.x, pt1.y),
             cv::Point(pt2.x, pt2.y), cv::Scalar(0, 255, 0), 2, CV_AA);
  }
  cv::imshow("auto_key_lines", show_cluster_image_);
#endif

  //  TestVanishingPoint(key_lane_lines, false);
  //  std::cout << "pri_vp_out:" << key_lane_lines.size() << std::endl;
  ExcludeOutLiersLines(key_lane_lines);
  //  TestVanishingPoint(key_lane_lines, true);
  //  std::cout << "vp_out:" << key_lane_lines.size() << std::endl;

  int len = key_lane_lines.size();
  if (len >= 2) {
    for (int i = 0; i < len - 1; i++) {
      if (key_lane_lines[i].ptDownExtendPoint.x >
          kImageWidth / 2 + kRoiOffset) {
        continue;
      }
      for (int j = 0 /*i + 1*/; j < len; j++) {
        if (key_lane_lines[j].ptDownExtendPoint.x <
            kImageWidth / 2 - kRoiOffset) {
          continue;
        }
        SetAutoLdVps(key_lane_lines[i].ptUpExtendPoint,
                     key_lane_lines[i].ptDownExtendPoint,
                     key_lane_lines[j].ptUpExtendPoint,
                     key_lane_lines[j].ptDownExtendPoint);
      }
    }
  }
}

std::vector<LaneLine> LaneModelFitting::Update(DataFrame* data_frame,
                                               cv::Mat& pm_black) {
  SetDataFrameToClass(data_frame, pm_black);
  //  data_frame_ = data_frame;

  std::vector<cv::Vec4i> hough_lines =
      GetFieldLinesV0(middle_start_line_, end_line_);

  std::vector<KeyLine> key_lane_lines = ClusterLine(hough_lines);

  //  TestVanishingPoint(key_lane_lines, false);
  //  std::cout << "pri_vp_out:" << key_lane_lines.size() << std::endl;
  ExcludeOutLiersLines(key_lane_lines);
  //  TestVanishingPoint(key_lane_lines, true);
  //  std::cout << "vp_out:" << key_lane_lines.size() << std::endl;

  std::vector<KeyLine> near_lanelines;
  std::vector<KeyLine> middle_lanelines;
  for (int i = 0; i < key_lane_lines.size(); i++) {
    if (key_lane_lines[i].field_id) {
      near_lanelines.push_back(key_lane_lines[i]);
    } else {
      middle_lanelines.push_back(key_lane_lines[i]);
    }
  }

#ifdef DEBUG_TEST
  for (int i = 0; i < near_lanelines.size(); i++) {
    cv::Point pt1 = near_lanelines[i].ptUpExtendPoint;    // ptUpPoint;
    cv::Point pt2 = near_lanelines[i].ptDownExtendPoint;  // ptDownPoint;
    cv::line(show_cluster_image_, cv::Point(pt1.x, pt1.y),
             cv::Point(pt2.x, pt2.y), cv::Scalar(0, 255, 0), 2, CV_AA);
  }
  for (int i = 0; i < middle_lanelines.size(); i++) {
    cv::Point pt1 = middle_lanelines[i].ptUpExtendPoint;    // ptUpPoint;
    cv::Point pt2 = middle_lanelines[i].ptDownExtendPoint;  // ptDownPoint;
    cv::line(show_cluster_image_, cv::Point(pt1.x, pt1.y),
             cv::Point(pt2.x, pt2.y), cv::Scalar(255, 0, 0), 2, CV_AA);
  }
  cv::imshow("show_cluster_image", show_cluster_image_);
#endif

  if (pri_lane_lines.size() > 0) {
    pri_near_lines_ =
        FrameAssociation(near_lanelines, start_line_, end_line_, 0);
    pri_middle_lines_ =
        FrameAssociation(middle_lanelines, middle_start_line_, start_line_, 1);
  } else {
    for (int t = 0; t < near_lanelines.size(); t++)
      near_lanelines[t].nIndex = lindex++;

    for (int t = 0; t < middle_lanelines.size(); t++)
      middle_lanelines[t].nIndex = lindex++;

    pri_near_lines_ = near_lanelines;
    pri_middle_lines_ = middle_lanelines;
    // bResPriIsSave = true;
  }

  std::vector<LaneLine> line_results =
      NearMiddleFieldCombine(pri_near_lines_, pri_middle_lines_);

#ifdef DEBUG_TEST
  TestNearMiddleFieldCombine(line_results);
#endif

  //  std::cout << "combine_result:" << line_results.size() << std::endl;
  pri_lane_lines = lane_kalman_->Update(data_frame, line_results);

//  for (int i = 0; i < temp_lane_lines.size(); i++) {
//    LaneLine temp_line = temp_lane_lines[i];
//    if (temp_line) {
//      pri_lane_lines.push_back(temp_line);
//    }
//  }

#ifdef DEBUG_TEST
  TestEkfResult(pri_lane_lines, line_results);
#endif
  //  std::cout <<"pri_lane_lines"<< pri_lane_lines.size() << std::endl;

  return pri_lane_lines;
}

std::vector<cv::Vec4i> LaneModelFitting::GetFieldLinesV0(int start_rows,
                                                         int end_rows) {
  uchar* src_black_image = pm_black_image.data;
  cv::Mat black_image(image_height, image_width, CV_8UC1, cv::Scalar(0, 0, 0));
  uchar* pBlackImage = black_image.data;
  //  int remove_number = 0;
  for (int i = start_rows; i < end_rows; ++i) {
    int i_index = i * image_width;
    for (int j = 0; j < image_width; ++j) {
      if (data_frame_->virtual_center_lines.sflag ==
          kCurrentLineIndexLeftAndRight) {
        int step = std::max((int)data_frame_->lane_width_limit[i].y >> 2, 1);
        cv::Point pt1 = data_frame_->virtual_center_lines.pixel[0];
        cv::Point pt2 = data_frame_->virtual_center_lines.pixel[1];
        if (i < start_line_) {
          pt1 = data_frame_->virtual_center_lines.pixel[2];
        }
        float center_j = CalcuateTools::GetInstance()->GetXorY(pt1, pt2, i, 1);

        //        std::cout << "step::" << step << std::endl;
        if (j > center_j - step && j < center_j + step) {
          //          if (src_black_image[i_index + j] == 255)
          //            std::cout << "remove_number::" << ++remove_number <<
          //            std::endl;
          continue;
        }
        if (src_black_image[i_index + j] == 255) {
          pBlackImage[i_index + j] = 255;
        }
      } else {
        if (src_black_image[i_index + j] == 255) {
          pBlackImage[i_index + j] = 255;
        }
      }
    }
  }

#ifdef DEBUG_BLACKPM
  cv::imshow("BlackPM_near", black_image);
#endif

  std::vector<cv::Vec4i> lines;
  CustomHoughLineP* custom_hough_line = new CustomHoughLineP();

  custom_hough_line->buuHoughLinesP(black_image, lines, kFitLineMinVote,
                                    kFitLineMinLength, kFitLineMaxGap,
                                    start_rows, end_rows);

#ifdef DEBUG_HOUGH
  Mat hough_imshow_;
  show_origin_image_.copyTo(hough_imshow_);
  for (int i = 0; i < lines.size(); i++) {
    cv::Point pt1 = cv::Point(lines[i](0), lines[i](1));
    cv::Point pt2 = cv::Point(lines[i](2), lines[i](3));
    cv::line(black_image, pt1, pt2, cv::Scalar(255, 255, 255), 2, CV_AA);
  }
  cv::imshow("hough_near", black_image);
#endif

  return lines;
}

std::vector<LaneLine> LaneModelFitting::NearMiddleFieldCombine(
    std::vector<KeyLine>& near_lanelines,
    std::vector<KeyLine>& middle_lanelines) {
  int nT = lane_line_fitting_width_[start_line_];  // 50;
  double d_Theth = 30;

  std::sort(near_lanelines.begin(), near_lanelines.end(),
            SortByDownExtendPointX);

  std::sort(middle_lanelines.begin(), middle_lanelines.end(),
            SortByDownExtendPointX);

  const int nNearSize = near_lanelines.size();
  const int nMidSize = middle_lanelines.size();

  int mnArray[nNearSize * nMidSize];
  bool siteM[nMidSize];

  for (int k = 0; k < nMidSize; ++k) {
    siteM[k] = true;
  }

  for (int i = 0; i < nNearSize; ++i) {
    int nearX = near_lanelines[i].ptUpExtendPoint.x;
    for (int j = 0; j < nMidSize; ++j) {
      int midX = middle_lanelines[j].ptDownExtendPoint.x;
      int nTempDis = abs(nearX - midX);

      double dTheth = CalcuateTools::GetInstance()->GetDoubleLineAngle(
          near_lanelines[i].fK, middle_lanelines[j].fK);

      if (nTempDis < nT && dTheth < d_Theth) {
        float fTheth = 1.f;
        switch (int(d_Theth) / 10) {
          case 0:
            fTheth = 1.f;
            break;
          case 1:
            fTheth = 1.2f;
            break;
          case 2:
            fTheth = 1.5f;
            break;
          default:
            fTheth = 2.f;
            break;
        }
        mnArray[i * nMidSize + j] = fTheth * nTempDis;
      } else {
        mnArray[i * nMidSize + j] = -1;
      }
    }
  }

  cv::Point zeroPointU(0, middle_start_line_);
  cv::Point zeroPointM(0, start_line_);
  cv::Point zeroPointD(0, end_line_);

  std::vector<LaneLine> tmpVecKeyLine;

  for (int i = 0; i < nNearSize;) {
    bool bIsMinValue = true;
    LaneLine tmpKeyLine;
    tmpKeyLine.factor[0] = near_lanelines[i].fK;
    tmpKeyLine.factor[1] = near_lanelines[i].fB;
    tmpKeyLine.factor[2] = 0;
    tmpKeyLine.factor[3] = 0;
    tmpKeyLine.near_field[0] = near_lanelines[i].ptUpPoint;
    tmpKeyLine.near_field[1] = near_lanelines[i].ptDownPoint;
    tmpKeyLine.near_field[2] = near_lanelines[i].ptUpExtendPoint;
    tmpKeyLine.near_field[3] = near_lanelines[i].ptDownExtendPoint;
    tmpKeyLine.nDistance[0] = near_lanelines[i].nDistance;

    float temp_x =
        tmpKeyLine.factor[0] * middle_start_line_ + tmpKeyLine.factor[1];
    tmpKeyLine.nDistance[1] = 0;
    tmpKeyLine.middle_field[0] = zeroPointU;
    tmpKeyLine.middle_field[1] = zeroPointM;
    tmpKeyLine.middle_field[2] = cv::Point(temp_x, end_line_);
    tmpKeyLine.middle_field[3] = tmpKeyLine.near_field[2];

    tmpKeyLine.nIndex = near_lanelines[i].nIndex;
    int nIndexNear = near_lanelines[i].nIndex;

    int nMinIndex = -1;
    int nMinDis = INT_MAX;
    int i_index = i * nMidSize;
    for (int j = 0; j < nMidSize; ++j) {
      if (siteM[j] == false) {
        mnArray[i_index + j] = -1;
      }

      int tempDis = mnArray[i_index + j];
      if (tempDis != -1 && nMinDis > tempDis) {
        nMinDis = tempDis;
        nMinIndex = j;
      }
    }

    if (nMinIndex == -1) {
      double midX =
          middle_start_line_ * tmpKeyLine.factor[0] + tmpKeyLine.factor[1];
      tmpKeyLine.middle_field[2] = cv::Point(midX, middle_start_line_);
      tmpKeyLine.middle_field[3] = tmpKeyLine.near_field[2];
    } else if (nMinIndex >= 0 && nMinIndex < nMidSize) {
      int nIndexMiddle = middle_lanelines[nMinIndex].nIndex;
      if (nIndexNear != nIndexMiddle) {
        for (int k = i + 1; k < nNearSize; ++k) {
          int tempDis = mnArray[k * nMidSize + nMinIndex];
          if (tempDis != -1 && tempDis < nMinDis) {
            bIsMinValue = false;
            break;
          }
        }
      }
      if (nIndexNear != nIndexMiddle && !bIsMinValue) {
        mnArray[i_index + nMinIndex] = -1;
      } else if (nIndexNear == nIndexMiddle || bIsMinValue) {
        siteM[nMinIndex] = false;
        tmpKeyLine.nDistance[1] = middle_lanelines[nMinIndex].nDistance;

        tmpKeyLine.factor[2] = middle_lanelines[nMinIndex].fK;
        tmpKeyLine.factor[3] = middle_lanelines[nMinIndex].fB;
        tmpKeyLine.middle_field[0] = middle_lanelines[nMinIndex].ptUpPoint;
        tmpKeyLine.middle_field[1] = middle_lanelines[nMinIndex].ptDownPoint;
        tmpKeyLine.middle_field[2] =
            middle_lanelines[nMinIndex].ptUpExtendPoint;
        tmpKeyLine.middle_field[3] =
            middle_lanelines[nMinIndex].ptDownExtendPoint;

        int index = nIndexNear;
        if (tmpKeyLine.nIndex > middle_lanelines[nMinIndex].nIndex &&
            middle_lanelines[nMinIndex].nIndex >= 0) {
          index = middle_lanelines[nMinIndex].nIndex;
        }

        tmpKeyLine.nIndex = index;
        near_lanelines[i].nIndex = index;
        middle_lanelines[nMinIndex].nIndex = index;
      }
    }

    if (bIsMinValue) {
      tmpVecKeyLine.push_back(tmpKeyLine);
      i++;
    }
  }

  for (int j = 0; j < nMidSize; ++j) {
    if (siteM[j]) {
      LaneLine tmpKeyLine;
      tmpKeyLine.factor[0] = 0;
      tmpKeyLine.factor[1] = 0;
      tmpKeyLine.factor[2] = middle_lanelines[j].fK;
      tmpKeyLine.factor[3] = middle_lanelines[j].fB;
      tmpKeyLine.middle_field[0] = middle_lanelines[j].ptUpPoint;
      tmpKeyLine.middle_field[1] = middle_lanelines[j].ptDownPoint;
      tmpKeyLine.middle_field[2] = middle_lanelines[j].ptUpExtendPoint;
      tmpKeyLine.middle_field[3] = middle_lanelines[j].ptDownExtendPoint;
      tmpKeyLine.nIndex = middle_lanelines[j].nIndex;

      tmpKeyLine.nDistance[0] = 0;
      tmpKeyLine.near_field[0] = zeroPointM;
      tmpKeyLine.near_field[1] = zeroPointD;

      double midX = end_line_ * tmpKeyLine.factor[2] + tmpKeyLine.factor[3];

      tmpKeyLine.near_field[3] = cv::Point(midX, end_line_);
      tmpKeyLine.near_field[2] = tmpKeyLine.middle_field[3];

      tmpKeyLine.nDistance[1] = middle_lanelines[j].nDistance;
      tmpVecKeyLine.push_back(tmpKeyLine);
    }
  }

  int nVecSize = tmpVecKeyLine.size();
  if (nVecSize > 1) {
    for (int i = 0; i < nVecSize - 1; ++i) {
      int nCurIndex = tmpVecKeyLine[i].nIndex;
      for (int j = i + 1; j < nVecSize; ++j) {
        int nTempIndex = tmpVecKeyLine[j].nIndex;
        if (nCurIndex == nTempIndex) {
          tmpVecKeyLine[j].nIndex = lindex++;
        }
      }

      if (lindex > kUnknowValue) {
        lindex = 1;
      }
    }
  }

  // refine center and remove lines
  std::vector<LaneLine> result_key_lines;
  for (int i = 0; i < tmpVecKeyLine.size(); i++) {
    if (abs(tmpVecKeyLine[i].near_field[3].x -
            tmpVecKeyLine[i].middle_field[2].x) > 2 * kImageWidth / 3) {
      continue;
    }
    //    int new_x = (tmpVecKeyLine[i].near_field[2].x +
    //                 tmpVecKeyLine[i].middle_field[3].x) >>
    //                1;

    //    tmpVecKeyLine[i].near_field[2].x = new_x;
    //    tmpVecKeyLine[i].middle_field[3].x = new_x;

    cv::Point2f top_point = CalcuateTools::GetInstance()->GetPointByDoublePoint(
        tmpVecKeyLine[i].near_field[2], tmpVecKeyLine[i].near_field[3],
        data_frame_->middle_start_line, 1);
    tmpVecKeyLine[i].middle_field[2] = cv::Point(top_point.x, top_point.y);
    tmpVecKeyLine[i].middle_field[3] = tmpVecKeyLine[i].near_field[2];

    result_key_lines.push_back(tmpVecKeyLine[i]);
  }

  return result_key_lines;
}

void LaneModelFitting::ClusterLineSubmodule(
    std::vector<KeyLine>& keylines_result) {
  const int nSize = keylines_result.size();
  if (nSize < 1) {
    return;
  }

  std::sort(keylines_result.begin(), keylines_result.end(),
            SortByDownExtendPointX);
  //  double dis_rho = 0;

  int nInCluter[nSize];
  for (int i = 0; i < nSize; ++i) {
    nInCluter[i] = -1;
  }

  int nCount = 0;
  nInCluter[0] = ++nCount;
  keylines_result[0].nCluter = nCount;

  if (nSize > 1) {
    for (int i = 1; i < nSize; ++i) {
      int up_extend_y = keylines_result[i].ptUpExtendPoint.y;
      int down_extend_y = keylines_result[i].ptDownExtendPoint.y;
      int dis_up_x = lane_line_fitting_width_[up_extend_y];
      int dis_down_x = lane_line_fitting_width_[down_extend_y];
      bool bFalg = false;
      int j = 1;
      while (i - j >= 0) {
        int temp_up_dis = abs(keylines_result[i].ptUpExtendPoint.x -
                              keylines_result[i - j].ptUpExtendPoint.x);
        int temp_down_dis = abs(keylines_result[i].ptDownExtendPoint.x -
                                keylines_result[i - j].ptDownExtendPoint.x);
        //                double fdisRho = abs(tmpVecKeyLines[i].fRho -
        //                tmpVecKeyLines[i - j].fRho);

        if (temp_up_dis < dis_up_x && temp_down_dis < dis_down_x) {
          int tmpnCluter = nInCluter[i - j];
          bool nFlag = true;
          for (int k = 1; k < i; ++k) {
            if (nInCluter[k] == tmpnCluter) {
              int nUpdis2 = abs(keylines_result[i].ptUpExtendPoint.x -
                                keylines_result[k].ptUpExtendPoint.x);
              int nDowndis2 = abs(keylines_result[i].ptDownExtendPoint.x -
                                  keylines_result[k].ptDownExtendPoint.x);
              if (nUpdis2 > dis_up_x || nDowndis2 > dis_down_x) {
                nFlag = false;
                break;
              }
            }
          }
          if (nFlag) {
            nInCluter[i] = tmpnCluter;
            keylines_result[i].nCluter = tmpnCluter;
            bFalg = true;
            break;
          }
        }
        j++;
      }
      if (!bFalg) {
        nInCluter[i] = ++nCount;
        keylines_result[i].nCluter = nCount;
      }
    }
  }

  // 2017.7.12 modify by LC
  for (int nIndex = 1; nIndex <= nCount; ++nIndex) {
    int nCur = 0;
    int bFlag = false;
    for (int i = 0; i < nSize; ++i) {
      int nMax = 0;
      if (nInCluter[i] == nIndex) {
        if (nMax < keylines_result[i].nDistance) {
          nMax = keylines_result[i].nDistance;
          nCur = i;
          bFlag = true;
        }
      }
    }
    if (bFlag) {
      keylines_result[nCur].bIsgoodLine = true;
    }
  }
}

std::vector<KeyLine> LaneModelFitting::AutoClusterLine(
    std::vector<cv::Vec4i>& lines) {
  std::vector<KeyLine> keylines_result;

  cv::Point pt_up_extend_point(0, 0);
  cv::Point pt_down_extend_point(0, 0);

  double line_fk = 0.0;
  double line_fb = 0.0;
  double line_frho = 0.0;
  int line_distance = 0.0;
  float fit_limit_rho_max = std::min(20.0f, 4 * kFitLimitRho);

  short t_index = 0;
  const int lines_size = lines.size();

  for (int i = 0; i < lines_size; ++i) {
    KeyLine temp_keyline;
    cv::Point pt1(lines[i][0], lines[i][1]);
    cv::Point pt2(lines[i][2], lines[i][3]);

    if (pt1.y > pt2.y) {
      std::swap(pt1, pt2);
    }

    if (abs(pt1.y - pt2.y) < 1) {
      line_fk = kUnknowValue;
      line_fb = 1.0 * pt1.x;
      line_frho = 90.0;
      line_distance = abs(pt2.y - pt1.y);
      pt_up_extend_point.x = pt1.x;
      pt_up_extend_point.y = middle_start_line_;
      pt_down_extend_point.x = pt1.x;
      pt_down_extend_point.y = end_line_;

    } else {
      CalcuateTools::GetInstance()->CalcLineKBR(pt1, pt2, line_fk, line_fb,
                                                line_frho, line_distance);
      if (line_frho < 0) {
        line_frho = 180 + line_frho;
      }
      pt_up_extend_point.x = middle_start_line_ * line_fk + line_fb;
      pt_down_extend_point.x = end_line_ * line_fk + line_fb;
      pt_up_extend_point.y = middle_start_line_;
      pt_down_extend_point.y = end_line_;
    }

    if (line_frho <= 180 - fit_limit_rho_max && line_frho > fit_limit_rho_max) {
      int line_len = abs(pt_up_extend_point.x - pt_down_extend_point.x);
      if (line_len < 0.4 * kImageWidth) {
        // tmpKeyLines.nCluter = 0;
        temp_keyline.fK = line_fk;
        temp_keyline.fB = line_fb;
        temp_keyline.fRho = line_frho;
        temp_keyline.nDistance = line_distance;
        temp_keyline.ptUpPoint = pt1;
        temp_keyline.ptDownPoint = pt2;
        temp_keyline.ptUpExtendPoint = pt_up_extend_point;
        temp_keyline.ptDownExtendPoint = pt_down_extend_point;
        temp_keyline.nIndex = t_index++;
        temp_keyline.nCluter = -1;
        temp_keyline.bIsgoodLine = false;
        keylines_result.push_back(temp_keyline);
      }
    }
  }

  ClusterLineSubmodule(keylines_result);

  std::vector<KeyLine> key_lines;
  int vecSize = keylines_result.size();
  for (int i = 0; i < vecSize; ++i) {
    if (keylines_result[i].bIsgoodLine) {
      key_lines.push_back(keylines_result[i]);
    }
  }

  return key_lines;
}

std::vector<KeyLine> LaneModelFitting::ClusterLine(
    std::vector<cv::Vec4i>& lines) {
  std::vector<KeyLine> keylines_result;

  cv::Point pt_up_extend_point(0, 0);
  cv::Point pt_down_extend_point(0, 0);

  double line_fk = 0.0;
  double line_fb = 0.0;
  double line_frho = 0.0;
  int line_distance = 0.0;
  int fit_limit_rho_max = 180.0 - kFitLimitRho;

  short t_index = 0;
  const int lines_size = lines.size();

  for (int i = 0; i < lines_size; ++i) {
    KeyLine temp_keyline;
    cv::Point pt1(lines[i][0], lines[i][1]);
    cv::Point pt2(lines[i][2], lines[i][3]);

    if (pt1.y > pt2.y) {
      std::swap(pt1, pt2);
    }

    bool near_field_flag = true;

    if (abs(pt1.y - pt2.y) < 1) {
      line_fk = kUnknowValue;
      line_fb = 1.0 * pt1.x;
      line_frho = 90.0;
      line_distance = abs(pt2.y - pt1.y);

      if (pt2.y < start_line_) {
        pt_up_extend_point.x = pt1.x;
        pt_up_extend_point.y = middle_start_line_;
        pt_down_extend_point.x = pt1.x;
        pt_down_extend_point.y = start_line_;
        near_field_flag = false;
      } else {
        pt_up_extend_point.x = pt1.x;
        pt_up_extend_point.y = start_line_;
        pt_down_extend_point.x = pt1.x;
        pt_down_extend_point.y = end_line_;
      }

    } else {
      CalcuateTools::GetInstance()->CalcLineKBR(pt1, pt2, line_fk, line_fb,
                                                line_frho, line_distance);
      if (line_frho < 0) {
        line_frho = 180 + line_frho;
      }
      if (pt2.y < start_line_) {
        pt_up_extend_point.x = middle_start_line_ * line_fk + line_fb;
        pt_down_extend_point.x = start_line_ * line_fk + line_fb;
        pt_up_extend_point.y = middle_start_line_;
        pt_down_extend_point.y = start_line_;
        near_field_flag = false;
      } else {
        pt_up_extend_point.x = start_line_ * line_fk + line_fb;
        pt_down_extend_point.x = end_line_ * line_fk + line_fb;
        pt_up_extend_point.y = start_line_;
        pt_down_extend_point.y = end_line_;
      }
    }

    if (line_frho <= fit_limit_rho_max && line_frho > kFitLimitRho) {
      int line_len = abs(pt_up_extend_point.x - pt_down_extend_point.x);
      if (line_len < 0.5 * kImageWidth) {
        int bottom_x = pt_down_extend_point.x;
        if ((bottom_x < 0.5 * kImageWidth + kRoiOffset &&
             line_frho > 90 - kFitLimitRho) ||
            (bottom_x > 0.5 * kImageWidth - kRoiOffset &&
             line_frho < 90 + kFitLimitRho)) {
          // tmpKeyLines.nCluter = 0;
          temp_keyline.fK = line_fk;
          temp_keyline.fB = line_fb;
          temp_keyline.fRho = line_frho;
          temp_keyline.nDistance = line_distance;
          temp_keyline.ptUpPoint = pt1;
          temp_keyline.ptDownPoint = pt2;
          temp_keyline.ptUpExtendPoint = pt_up_extend_point;
          temp_keyline.ptDownExtendPoint = pt_down_extend_point;
          temp_keyline.nIndex = t_index++;
          temp_keyline.nCluter = -1;
          temp_keyline.bIsgoodLine = false;
          temp_keyline.field_id = near_field_flag;
          keylines_result.push_back(temp_keyline);
        }
      }
    }
  }

  ClusterLineSubmodule(keylines_result);

  std::vector<KeyLine> key_lines;
  int vecSize = keylines_result.size();
  for (int i = 0; i < vecSize; ++i) {
    if (keylines_result[i].bIsgoodLine) {
      key_lines.push_back(keylines_result[i]);
    }
  }

  return key_lines;
}

std::vector<KeyLine> LaneModelFitting::FrameAssociation(
    std::vector<KeyLine>& cur_keylines, int nmin, int nmax, int nindex) {
  float nExtandScale = 1.2f;
  std::vector<KeyLine> tmpVecKeyLine;
  const int n = pri_lane_lines.size();
  const int m = cur_keylines.size();
  if (m < 1) return tmpVecKeyLine;

  float nDix = 1.0;
  float up_distance_limit = nExtandScale * lane_line_fitting_width_[nmin];
  float down_distance_limit = nExtandScale * lane_line_fitting_width_[nmax];

  int pIndex[m];
  int pValue[m];
  int mnArray[m * n];

  for (int j = 0; j < m; ++j) {
    pIndex[j] = -1;
    pValue[j] = -1;
  }

  cv::Point ptUpExtendPoint, ptDownExtendPoint;
  cv::Point ptPriUpExtendPoint, ptPriDownExtendPoint;
  for (int i = 0; i < m; ++i) {
    ptUpExtendPoint = cur_keylines[i].ptUpExtendPoint;
    ptDownExtendPoint = cur_keylines[i].ptDownExtendPoint;
    float current_rho = CalcuateTools::GetInstance()->CalRho(ptUpExtendPoint,
                                                             ptDownExtendPoint);
    for (int j = 0; j < n; ++j) {
      if (nindex == 0) {
        ptPriUpExtendPoint = pri_lane_lines[j].Line_now[0];
        ptPriDownExtendPoint = pri_lane_lines[j].Line_now[1];
      } else {
        ptPriUpExtendPoint = pri_lane_lines[j].Line_now[2];
        ptPriDownExtendPoint = pri_lane_lines[j].Line_now[3];
      }
      int nUpDis = abs(ptUpExtendPoint.x - ptPriUpExtendPoint.x);
      int nDownDis = abs(ptDownExtendPoint.x - ptPriDownExtendPoint.x);

      float pri_rho = CalcuateTools::GetInstance()->CalRho(
          ptPriUpExtendPoint, ptPriDownExtendPoint);
      if (nUpDis < up_distance_limit && nDownDis < down_distance_limit &&
          abs(current_rho - pri_rho) < kControlMinRho) {
        int nTmp = abs(nDix * nUpDis + nDownDis);
        mnArray[i * n + j] = nTmp;
      } else {
        mnArray[i * n + j] = -1;
      }
    }
  }

  for (int j = 0; j < n; ++j) {
    int nMin = INT_MAX;
    int jk = -1;
    for (int i = 0; i < m; ++i) {
      int mnValue = mnArray[i * n + j];
      if (mnValue != -1 && nMin > mnValue) {
        nMin = mnValue;
        jk = i;
      }
    }
    if (jk >= 0 && jk < m) {
      if (pValue[jk] == -1 || (pValue[jk] != -1 && nMin < pValue[jk])) {
        pIndex[jk] = j;
        pValue[jk] = nMin;
      }
    }
  }

  KeyLine tmpKeyLine;
  for (int k = 0; k < m; ++k) {
    tmpKeyLine = cur_keylines[k];
    if (pIndex[k] == -1) {
      tmpKeyLine.nIndex = lindex;
      lindex++;
    } else {
      tmpKeyLine.nIndex = pri_lane_lines[pIndex[k]].nIndex;
    }
    tmpVecKeyLine.push_back(tmpKeyLine);
  }
  if (lindex > kUnknowValue) {
    lindex = 1;
  }

  return tmpVecKeyLine;
}

void LaneModelFitting::ExcludeOutLiersLines(std::vector<KeyLine>& key_lines) {
  int length = key_lines.size();
  if (length < 1) {
    return;
  }
  cv::Point t_vanishing_point = constant_vanishing_point;
  int extend_size = 1;
  int nMax = extend_size * std::min(kVanishPointLengthX, kVanishPointLengthY);
  if (length == 1) {
    cv::Point pt1 = key_lines[0].ptUpPoint;
    cv::Point pt2 = key_lines[0].ptDownPoint;
    double temp = CalcuateTools::GetInstance()->PointToLineDistByPoint(
        t_vanishing_point, pt1, pt2);
    if (temp > nMax * nMax) {
      key_lines.clear();
    }
    return;
  }

  for (int i = 0; i < length; ++i) {
    key_lines[i].bIsUse = false;
  }

  std::vector<cv::Point> line_index;
  std::vector<cv::Point2f> cross_point;
  for (int i = 0; i < length - 1; i++) {
    for (int j = i + 1; j < length; j++) {
      cv::Point2f pt = CalcuateTools::GetInstance()->GetCrossPoint(
          key_lines[i].fK, key_lines[i].fB, key_lines[j].fK, key_lines[j].fB);
      if (pt.x == 0 && pt.y == 0) {
        continue;
      } else {
        line_index.push_back(cv::Point(i, j));
        cross_point.push_back(pt);
      }
    }
  }

  int maxX = t_vanishing_point.x + extend_size * kVanishPointLengthX;
  int minX = t_vanishing_point.x - extend_size * kVanishPointLengthX;
  int maxY = t_vanishing_point.y + extend_size * kVanishPointLengthY;
  int minY = t_vanishing_point.y - extend_size * kVanishPointLengthY;

  int nCount = 0;
  for (int i = 0; i < cross_point.size(); ++i) {
    cv::Point2f pt = cross_point[i];
    if ((pt.x < maxX && pt.x > minX) && (pt.y < maxY && pt.y > minY)) {
      int line_1 = line_index[i].x;
      int line_2 = line_index[i].y;
      if (line_1 < length && line_1 >= 0) {
        key_lines[line_1].bIsUse = true;
      }
      if (line_2 < length && line_2 >= 0) {
        key_lines[line_2].bIsUse = true;
      }
      nCount++;
    }
  }

  if (nCount == 0) {
    t_vanishing_point = adapt_vanishing_point;
    extend_size = 0.8;
    maxX = t_vanishing_point.x + extend_size * kVanishPointLengthX;
    minX = t_vanishing_point.x - extend_size * kVanishPointLengthX;
    maxY = t_vanishing_point.y + extend_size * kVanishPointLengthY;
    minY = t_vanishing_point.y - extend_size * kVanishPointLengthY;
    for (int i = 0; i < cross_point.size(); ++i) {
      cv::Point2f pt = cross_point[i];
      if ((pt.x < maxX && pt.x > minX) && (pt.y < maxY && pt.y > minY)) {
        int line_1 = line_index[i].x;
        int line_2 = line_index[i].y;
        if (line_1 < length && line_1 >= 0) {
          key_lines[line_1].bIsUse = true;
        }
        if (line_2 < length && line_2 >= 0) {
          key_lines[line_2].bIsUse = true;
        }
        nCount++;
      }
    }
    if (nCount != 0) {
      adapt_vanishing_point = constant_vanishing_point;
    }
  }

  std::vector<KeyLine> out_key_line;
  for (int t = 0; t < length; ++t) {
    if (key_lines[t].bIsUse) {
      out_key_line.push_back(key_lines[t]);
    }
  }
  key_lines = out_key_line;
}

void LaneModelFitting::TestVanishingPoint(std::vector<KeyLine>& key_lines,
                                          bool bflag) {
  cv::Mat showImg;
  show_origin_image_.copyTo(showImg);
  int len = key_lines.size();

  for (int i = 0; i < len - 1; i++) {
    for (int j = i + 1; j < len; j++) {
      cv::Point2f pt = CalcuateTools::GetInstance()->GetCrossPoint(
          key_lines[i].ptUpExtendPoint, key_lines[i].ptDownExtendPoint,
          key_lines[j].ptUpExtendPoint, key_lines[j].ptDownExtendPoint);
      if (pt.x == 0 && pt.y == 0) continue;
      cv::line(showImg, pt, key_lines[i].ptDownExtendPoint,
               cv::Scalar(255, 0, 255), 1, CV_AA);
      cv::line(showImg, pt, key_lines[j].ptDownExtendPoint,
               cv::Scalar(255, 0, 255), 1, CV_AA);
      cv::circle(showImg, pt, 1, cv::Scalar(0, 0, 255), 5);
    }
  }

  int maxX = adapt_vanishing_point.x + kVanishPointLengthX;
  int minX = adapt_vanishing_point.x - kVanishPointLengthX;
  int maxY = adapt_vanishing_point.y + kVanishPointLengthY;
  int minY = adapt_vanishing_point.y - kVanishPointLengthY;

  cv::line(showImg, cv::Point(maxX, 0), cv::Point(maxX, image_height),
           cv::Scalar(0, 255, 255), 1, CV_AA);
  cv::line(showImg, cv::Point(minX, 0), cv::Point(minX, image_height),
           cv::Scalar(0, 255, 255), 1, CV_AA);
  cv::line(showImg, cv::Point(0, maxY), cv::Point(image_width, maxY),
           cv::Scalar(0, 255, 255), 1, CV_AA);
  cv::line(showImg, cv::Point(0, minY), cv::Point(image_width, minY),
           cv::Scalar(0, 255, 255), 1, CV_AA);

  cv::line(showImg, cv::Point(0, start_line_),
           cv::Point(image_width, start_line_), cv::Scalar(255, 255, 255), 2,
           CV_AA);
  cv::line(showImg, cv::Point(0, middle_start_line_),
           cv::Point(image_width, middle_start_line_),
           cv::Scalar(255, 255, 255), 2, CV_AA);
  if (bflag) {
    cv::imshow("showImg", showImg);
  } else {
    cv::imshow("showImg_pri", showImg);
  }
}

void LaneModelFitting::TestFrameAssoctation() {
  cv::Mat showImage2;
  show_origin_image_.copyTo(showImage2);
  for (int i = 0; i < pri_near_lines_.size(); i++) {
    cv::Point UpPoint = pri_near_lines_[i].ptUpExtendPoint;
    cv::Point DownPoint = pri_near_lines_[i].ptDownExtendPoint;
    int nId = pri_near_lines_[i].nIndex;
    cv::line(showImage2, cv::Point(UpPoint.x, UpPoint.y),
             cv::Point(DownPoint.x, DownPoint.y),
             data_frame_->scalar_color[nId % 7], 2, CV_AA);
  }

  for (int i = 0; i < pri_middle_lines_.size(); i++) {
    cv::Point UpPoint = pri_middle_lines_[i].ptUpExtendPoint;
    cv::Point DownPoint = pri_middle_lines_[i].ptDownExtendPoint;
    int nId = pri_middle_lines_[i].nIndex;
    // if(g_vecMidPriSaveLine[i].bIsSpline ==false)
    cv::line(showImage2, cv::Point(UpPoint.x, UpPoint.y),
             cv::Point(DownPoint.x, DownPoint.y),
             data_frame_->scalar_color[nId % 7], 2, CV_AA);
    //        if (SHOW_INDEX)
    //            ShowTextinMat(showImage2, Point(UpPoint.x, UpPoint.y),
    //            g_vecMidPriSaveLine[i].nIndex);
  }
  for (int i = 0; i < pri_lane_lines.size(); i++) {
    cv::Point UpPoint = pri_lane_lines[i].Line_now[0];
    cv::Point DownPoint = pri_lane_lines[i].Line_now[1];

    cv::line(showImage2, cv::Point(UpPoint.x, UpPoint.y),
             cv::Point(DownPoint.x, DownPoint.y), data_frame_->scalar_color[10],
             1, CV_AA);

    UpPoint = pri_lane_lines[i].Line_now[2];
    DownPoint = pri_lane_lines[i].Line_now[3];
    cv::line(showImage2, cv::Point(UpPoint.x, UpPoint.y),
             cv::Point(DownPoint.x, DownPoint.y), data_frame_->scalar_color[10],
             1, CV_AA);

    cv::imshow("showImage2", showImage2);
  }
}

void LaneModelFitting::TestNearMiddleFieldCombine(
    std::vector<LaneLine>& line_results) {
  cv::Mat showCombineImage;
  show_origin_image_.copyTo(showCombineImage);
  for (int i = 0; i < line_results.size(); i++) {
    int nId = line_results[i].nIndex;
    //    if (line_results[i].nDistance[0] != 0)
    {
      cv::Point UpPointNow = line_results[i].near_field[2];
      cv::Point DownPointNow = line_results[i].near_field[3];
      cv::line(showCombineImage, UpPointNow, DownPointNow,
               data_frame_->scalar_color[nId % 7], 10,
               CV_AA);  // nId%7
    }

    //    if (line_results[i].nDistance[1] != 0)
    {
      cv::Point UpPointNow = line_results[i].middle_field[2];
      cv::Point DownPointNow = line_results[i].middle_field[3];
      cv::line(showCombineImage, UpPointNow, DownPointNow,
               data_frame_->scalar_color[nId % 7], 10, CV_AA);
      cv::Point indexPt = cv::Point(DownPointNow.x, DownPointNow.y);
      DebugShow::Instance()->ShowTextinMat(showCombineImage, indexPt, nId);
    }

    //        if (SHOW_INDEX)
  }
  cv::imshow("6NearMidFileCombine", showCombineImage);
}

void LaneModelFitting::TestEkfResult(std::vector<LaneLine>& ekf_line_results,
                                     std::vector<LaneLine>& line_results) {
  cv::Mat showPriSaveLine;
  show_origin_image_.copyTo(showPriSaveLine);

  for (int i = 0; i < line_results.size(); i++) {
    int nId = line_results[i].nIndex;
    //    if (line_results[i].nDistance[0] != 0)
    {
      cv::Point UpPointNow = line_results[i].near_field[2];
      cv::Point DownPointNow = line_results[i].near_field[3];
      cv::line(showPriSaveLine, UpPointNow, DownPointNow,
               data_frame_->scalar_color[nId % 7], 5,
               CV_AA);  // nId%7
    }

    //    if (line_results[i].nDistance[1] != 0)
    {
      cv::Point UpPointNow = line_results[i].middle_field[2];
      cv::Point DownPointNow = line_results[i].middle_field[3];
      cv::line(showPriSaveLine, UpPointNow, DownPointNow,
               data_frame_->scalar_color[nId % 7], 5, CV_AA);
      //      ShowTextinMat(showPriSaveLine, UpPointNow, nId);
    }

    //        if (SHOW_INDEX)
  }

  for (int i = 0; i < ekf_line_results.size(); i++) {
    int nId = ekf_line_results[i].nIndex;
    cv::Point UpPointNow =
        ekf_line_results[i].Line_now[0];  // Line_now  Line_now Line_current
    cv::Point DownPointNow = ekf_line_results[i].Line_now[1];
    cv::line(showPriSaveLine, UpPointNow, DownPointNow,
             data_frame_->scalar_color[8], 1, CV_AA);

    UpPointNow = ekf_line_results[i].Line_now[2];
    DownPointNow = ekf_line_results[i].Line_now[3];

    cv::line(showPriSaveLine, UpPointNow, DownPointNow,
             data_frame_->scalar_color[8], 1, CV_AA);
    DebugShow::Instance()->ShowTextinMat(showPriSaveLine, DownPointNow, nId);
  }
  //  namedWindow("TestEkfResult", CV_WINDOW_NORMAL);
  //  resizeWindow("TestEkfResult", gShowWidth, gShowHeight);
  imshow("TestEkfResult", showPriSaveLine);
  //  cv::waitKey(0);
}
}
